
pub const OS_NVIC_SYSPRI2:u32 = 0xE000ED20;
pub const OS_NVIC_PENDSV_PRI:u32 = 0xF0F00000;
pub const OS_NVIC_INT_CTRL:u32 = 0xE000ED04;
pub const OS_NVIC_PENDSVSET:u32 = 0x10000000;


pub unsafe extern "C" fn hal_start_to_run() {
	asm!(
	"
    ldr     r4, =OS_NVIC_SYSPRI2
    ldr     r5, =OS_NVIC_PENDSV_PRI
    str     r5, [r4]

    mov     r0, #2
    msr     CONTROL, r0

    ldr     r1, =g_losTask
    ldr     r0, [r1, #4]
    ldr     r12, [r0]
    add     r12, r12, #36

    ldmfd   r12!, {{r0-r7}}
    msr     psp, r12

    mov     lr, r5
    cpsie   I
    bx      r6
	"
	);
}



pub unsafe extern "C" fn hal_int_lock() {
	asm!(
	"
    MRS R0, PRIMASK
    CPSID I
    BX LR
	"
	);
}



pub unsafe extern "C" fn hal_int_unlock() {
	asm!(
	"
    MRS R0, PRIMASK
    CPSIE I
    BX LR
	"
	);
}


pub unsafe extern "C" fn hal_task_schedule() {
	asm!(
	"
    ldr     r0, =OS_NVIC_INT_CTRL
    ldr     r1, =OS_NVIC_PENDSVSET
    str     r1, [r0]

    dsb
    isb
    bx      lr
	"
	);
}


pub unsafe extern "C" fn hal_pendsv() {
	asm!(
	"
    mrs     r12, PRIMASK
    cpsid   I

HalTaskSwitch:
	// TODO Later
	//push    {{r12, lr}}
    //blx     OsSchedTaskSwitch
    //pop     {{r12, lr}}
    cmp     r0, #0
    mov     r0, lr
    bne     TaskContextSwitch
    msr     PRIMASK, r12
    bx      lr

TaskContextSwitch:
    mov     lr, r0
    mrs     r0, psp

	stmfd   r0!, {{r4-r12}}

    ldr     r5, =g_losTask
    ldr     r6, [r5]
    str     r0, [r6]

    ldr     r0, [r5, #4]
    str     r0, [r5]

    ldr     r1, [r0]

    ldmfd   r1!, {{r4-r12}}
    msr     psp,  r1

    msr     PRIMASK, r12

    bx      lr
	"
	);
}
